/************************************************************
**                                                         **
** FILE: Freescale_CUP_Functions.c                         **
**                                                         **
** DESCRIPTION:Specific functions needly in the Freescale  **
** cup                                                     **
**                                                         **
** VERSION: 1.0                                            **
**                                                         **
** AUTOR: UTCH-RAPTOR TEAM                                 **
**                                                         **
** DATE: 26/September/2012                                 **
**                                                         **
************************************************************/
#include "PAD_Defs.h"
#include "Constant_Defs.h"
#include "MPC5604B_M27V.h"
#include "EMIOS_0_conf.h"
#include "ADC_conf.h"
#include "Standard_Functions.h"
#include "Parallax_Library.h"

vuint8_t Final_Counter=0,Start_End_Aux=0;

vuint32_t Proportional_Var[5],Shift_Gear;
vuint32_t Error_Group=0,Correction;
vuint32_t Speed[5];
//Constants:
vuint32_t Black_Limit=150,Smart_Frame_Width=70;

float Error,Set_Point;

extern Black_Bar,Center_Position,Car_Direction,PWM_Center,Race_End,Car_Direction,Tokio_Drift;


/*********************************************
Function used when is detected a start/end  **
condition in the race                       **
*********************************************/
void Start_Detect(vuint8_t Times_For_Finish)
{
	if(Black_Bar==3 || Start_End_Aux==1)
	   { 
		   Final_Counter++;
		   Start_End_Aux=1;
	   }
	if(Final_Counter>=Times_For_Finish)
	  {
	     Race_End=ENABLE;	
	  }	
}
/*********************************************
Function used to calculate the correction   **
in the servomotor PWM                       **
*********************************************/
void Direction_Control(void)
{
   Error=Set_Point-Center_Position;
   if(Error<(-10))     
 	  {
	   	 Car_Direction=LEFT_TURN;
		 Error=Error/(-1);
	  }
  else if(Error>10)     
      {
	 	 Car_Direction=RIGHT_TURN;
	  }	  
  else if(Error>=(-10) && Error<0)     
      {
	 	 Car_Direction=LEFT_CENTER;
	 	 Error=Error/(-1);
	  }
  else if(Error<=10 && Error>0)     
      {
	 	 Car_Direction=RIGHT_CENTER;
	  }	  
/****************************************/	  
	if(Error>0 && Error<=10)
       {
          Error_Group=0;
          Tokio_Drift=DISABLE;           		
	   }
	if(Error>10 && Error<=20)
       {
          Error_Group=1;
          Error=Error-10;	
          Tokio_Drift=DISABLE;          	
	   }	   
	if(Error>20 && Error<=30)
       {
          Error_Group=2;
          Error=Error-20;		
          Tokio_Drift=DISABLE;          
	   }
	if(Error>30 && Error<=40)
       {
          Error_Group=3;
          Error=Error-30;
          Tokio_Drift=ENABLE;		
	   }
	if(Error>40 && Error<=45)
       {
          Error_Group=4;
          Error=Error-40;
          Tokio_Drift=ENABLE;          		
	   }
/****************************************/	   
 	      switch(Error_Group)
 	      {
 	      	case 0:
 	      	   Correction=Error*Proportional_Var[0];
 	      	   Shift_Gear=4;
 	      	break;
 	      	case 1:
 	      	   Correction=Error*Proportional_Var[1]+(Proportional_Var[0]*10);
 	      	   Shift_Gear=3;
 	      	break;
 	      	case 2:
 	      	   Correction=Error*Proportional_Var[2]+((Proportional_Var[0]*10)+(Proportional_Var[1]*10));
 	      	   Shift_Gear=2;
 	      	break;
 	      	case 3:
 	      	   Correction=Error*Proportional_Var[3]+((Proportional_Var[0]*10)+(Proportional_Var[1]*10)+(Proportional_Var[2]*10));
 	      	   Shift_Gear=1;
 	      	break;
 	      	case 4:
 	      	   Correction=Error*Proportional_Var[4]+((Proportional_Var[0]*10)+(Proportional_Var[1]*10)+(Proportional_Var[2]*10)+(Proportional_Var[3]*10));
 	      	   Shift_Gear=0;
 	      	break;
 	      	default:
 	      	break;
 	      }
		  if(Car_Direction==LEFT_TURN || Car_Direction==LEFT_CENTER)
		     {
                DIRECTION_PWM=PWM_Center-Correction;
		     }
		  if(Car_Direction==RIGHT_TURN || Car_Direction==RIGHT_CENTER)
		     {
                DIRECTION_PWM=PWM_Center+Correction;
		     }	  
}
/*********************************************
Function used to control the speed of the   **
car depending on the direction control      **
*********************************************/
void Speed_Control(void)
{
	if(Race_End==ENABLE)
	{
	   L_MOTOR_SWITCH=DISABLE;
	   R_MOTOR_SWITCH=DISABLE;		
	}
	else if(Race_End==DISABLE)
	{ 
	   if(Car_Direction==RIGHT_CENTER || Car_Direction==LEFT_CENTER || Car_Direction==ABSOLUT_CENTER)
	      {
              L_MOTOR_SWITCH=ENABLE;
              R_MOTOR_SWITCH=ENABLE;
              LEFT_PWM=Speed[Shift_Gear];
              RIGHT_PWM=Speed[Shift_Gear];	   	
	      }
	   else if(Car_Direction==RIGHT_TURN)
	      {  
	   	      if(Tokio_Drift==ENABLE)
	   	         {
	   	            L_MOTOR_SWITCH=DISABLE;
	   	            R_MOTOR_SWITCH=ENABLE;
	   	            LEFT_PWM=Speed[Shift_Gear];
	   	            RIGHT_PWM=Speed[Shift_Gear];	   	      	
	   	         }
	   	      else if(Tokio_Drift==DISABLE)
	   	         {
	   	            L_MOTOR_SWITCH=ENABLE;
	   	            R_MOTOR_SWITCH=ENABLE;	   	
	   	            LEFT_PWM=Speed[Shift_Gear];
	   	            RIGHT_PWM=Speed[Shift_Gear];	   	                  	
	   	         }
	      }
	   else if(Car_Direction==LEFT_TURN)
	      {
	   	      if(Tokio_Drift==ENABLE)
	   	         {
	   	            L_MOTOR_SWITCH=ENABLE;
	   	            R_MOTOR_SWITCH=DISABLE;	   	
	   	            LEFT_PWM=Speed[Shift_Gear];
	   	            RIGHT_PWM=Speed[Shift_Gear];	   	                  	
	   	         }
	   	      else if(Tokio_Drift==DISABLE)
	   	         {
	   	            L_MOTOR_SWITCH=ENABLE;
	   	            R_MOTOR_SWITCH=ENABLE;	   	
	   	            LEFT_PWM=Speed[Shift_Gear];
	   	            RIGHT_PWM=Speed[Shift_Gear];	   	                  	
	   	         }	   	         	   	
	      }		
	}
}
/*********************************************
Function used to select between diferents   **
speeds and proportional constants           **
*********************************************/
void Choose_Program(void)
{
   vuint32_t Program_Choosed=0;
   Program_Choosed=Set_Number(300);//Delay of 300ms between binary displays
   switch(Program_Choosed)
      {
      	    	      	    	      	      	      	
      }
	
}
/*********************************************
Delay function used in the start, a count   **
down is displayed in the LEDS               **
*********************************************/
void Ready_Set_Go(vuint32_t Delay_In_ms)
{
	Binary_Display(15);
	Delay_ms(Delay_In_ms);
	Binary_Display(7);
	Delay_ms(Delay_In_ms);
	Binary_Display(3);
	Delay_ms(Delay_In_ms);
	Binary_Display(1);
	Delay_ms(Delay_In_ms);
	Binary_Display(0);			
}
/*********************************************
Function to wait the start of the 20ms      **
EMIOS_0 channel                             **
*********************************************/
void Wait_Start(void)
{
	while(FLAG_20ms!=1)
	{}
	FLAG_20ms=1;
}
/*********************************************
Main function of the smart car, including   **
the taking picture with the camera          **
*********************************************/
void Smart_Car_Control(void)
{
   vuint8_t i=0;
	while(PUSH_4==1)
	{
	   Wait_Start();
       Speed_Control();
       for(i=0;i<=11;i++)
          {
	         Take_Picture(1,Black_Limit,Smart_Frame_Width);
 	         Start_Detect(5); 	         
          }	    	    	   	    	   
	   Direction_Control();
	
	}
	
}